#include "SMARTStateEquationSolveLib.h"

int main(int argc,char *argv[])
{		
	//Initialize
	double *A = NULL, *B = NULL, *C = NULL, *D = NULL;
	T_SRC *tSrc = NULL;
	FILE *recordData = NULL;
	
	if((recordData = fopen(SOLUTIONDATA, "w+")) == NULL)
	{
		printf("File Creation/Reading Failed! Please Check For the Existance of the File!\n");
		exit(1);
	}

	if(A == NULL)
		A = SMARTNEW(double, 4);		
	if(B == NULL)
		B = SMARTNEW(double, 4);
	if(C == NULL)
		C = SMARTNEW(double, 2);
	if(D == NULL)
		D = NULL;

	/*A*/
	A[0] = -0.5572;
	A[1] = -0.7814;
	A[2] = 0.7814;
	A[3] = 0;
	/*B*/
	B[0] = 1;
	B[1] = -1;
	B[2] = 0;
	B[3] = 2;
	/*C*/
	C[0] = 1.9691;
	C[1] = 6.4493;
	
	//Real Effect Call
	SMARTSESSolve(A, B, C, D, tSrc, recordData);

	//Dispose
	if(A != NULL)
		SMARTDELETE(A);
	if(B != NULL)
		SMARTDELETE(B);
	if(C != NULL)
		SMARTDELETE(C);
	if(D != NULL)
		SMARTDELETE(D);

	T_SRC *tp = tSrc;
	while(tp != NULL)
	{
		T_SRC *ttp = tp;
		tp = tp->next;
		free(ttp);		
	}
	fclose(recordData);
	return 0;
}


/**** dX/dt = Ax + Bu ****/
/**** Y = Cx + Du ****/
void SMARTSESSolve(double *A, double *B, double *C, double *D, T_SRC *tSrc, FILE* fileHandler)
{
	int matrixScale;
	double *X = NULL;
	double Y = 0;
	double steps = 0;
	double hfactor = 0;	
	SES_SRC *sesSrc = NULL;
	
	SMARTSESInitializeData(A, B, C, D, tSrc, sesSrc, &X, &Y, &matrixScale, &steps, &hfactor);
	SMARTSESIterationSolve(A, B, C, D, sesSrc, &X, &Y, matrixScale, steps, hfactor, fileHandler);
	SMARTSESSolutionPrint(fileHandler);
	SMARTSESSolutionShow(fileHandler);
	SMARTSESDispose(sesSrc, X);
}
